#include "robotarrawkey.h"

RobotArrawKey::RobotArrawKey(){
    node=rclcpp::Node::make_shared("robotarrawkey"); //节点名
    _publisher =
        node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);  //创建发布
    this->start();
}


void RobotArrawKey::run(){
    rclcpp::WallRate loop_rate(1);
    while (rclcpp::ok())        //只做线程状态监测
    {
        rclcpp::spin_some(node);
        loop_rate.sleep();
    }
    rclcpp::shutdown();
}

void RobotArrawKey::up() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = 0.15;
    msg.angular.z = 0.0;
    _publisher->publish(msg);
}

void RobotArrawKey::down() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = -0.15;
    msg.angular.z = 0.0;
    _publisher->publish(msg);
}

void RobotArrawKey::left_up() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = 0.15;
    msg.angular.z = 0.15;
    _publisher->publish(msg);
}

void RobotArrawKey::right_up() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = 0.15;
    msg.angular.z = -0.15;
    _publisher->publish(msg);
}

void RobotArrawKey::right() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = 0.0;
    msg.angular.z = -0.15;
    _publisher->publish(msg);
}

void RobotArrawKey::left() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = 0.0;
    msg.angular.z = 0.15;
    _publisher->publish(msg);
}

void RobotArrawKey::left_down() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = -0.15;
    msg.angular.z = 0.15;
    _publisher->publish(msg);
}

void RobotArrawKey::right_down() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = -0.15;
    msg.angular.z = -0.15;
    _publisher->publish(msg);
}

void RobotArrawKey::stop() {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = 0.0;
    msg.angular.z = 0.0;
    _publisher->publish(msg);
}
